/*
 * TransitPath.java
 * 
 * Copyright 2008 David D. Emory
 * 
 * This file is part of Five Points. See <http://www.fpdev.org> for
 * additional information regarding the project.
 * 
 * Five Points is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * Five Points is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with Five Points.  If not, see <http://www.gnu.org/licenses/>.
 */
package org.fpdev.core.transit;

import java.awt.Color;
import org.fpdev.core.FPEngine;
import org.fpdev.core.basenet.BLink;
import org.fpdev.core.basenet.BNode;
import org.fpdev.core.basenet.Path;
import org.fpdev.util.*;

import java.io.*;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;

import org.fpdev.core.Scenario;
import org.fpdev.core.basenet.BaseNetwork;
import org.fpdev.apps.rtemaster.GUIOutputAcceptor;
import org.fpdev.apps.rtemaster.gui.map.MapCanvas;

public class TransitPath extends Path {

  /*public final static int WALK = -1;
  public final static int BUS = 1;
  public final static int METRO = 2;
  public final static int LIGHTRAIL = 3;
  public final static int REGIONALRAIL = 4;*/


  private SubRoute sub_;
  private List<Stop> stops_; //, addedStops_; //, dirPath_;
  //private List<CNode> addedStops_;
  private Path dirPath_;
  private Type type_;
  private boolean addStop_;
  private Color drawPathColor_,  drawStopsColor_;
  private boolean drawBasicStops_;

  public void rev() {
    Collections.reverse(path_);
  }

  public enum Type {
    BUS           ("Bus", "bus", Color.blue, 1),
    METRO         ("Metro", "met", Color.red, 2),
    LIGHTRAIL     ("Light Rail", "lrt", Color.orange, 3),
    REGIONALRAIL  ("Regional Rail", "rer", Color.magenta, 4);

    private final String name_, shortName_;
    private final Color color_;
    private final int code_;

    Type(String name, String shortName, Color color, int code) {
        name_ = name;
        shortName_ = shortName;
        color_ = color;
        code_ = code;
    }

    public String getName() { return name_; }

    @Override
    public String toString() { return getName(); }

    public String getShortName() { return shortName_; }

    public Color getColor() { return color_; }

    public int getFileCode() { return code_; }
  }

  public static Type typeFromFileCode(int code) {
    switch(code) {
      case 1: return Type.BUS;
      case 2: return Type.METRO;
      case 3: return Type.LIGHTRAIL;
      case 4: return Type.REGIONALRAIL;
    }
    return null;
  }
  
  /**
   * Constructs a new empty TransitPath.
   * 
   * @param engine A reference to the active FPEngine
   * @param sub The SubRoute that will contain this path
   * @param type The type (i.e. transit technology) for this path
   */
  public TransitPath(FPEngine engine, SubRoute sub, Type type) {
    this(engine, sub, type, null, false);
  }

  /**
   * Constructs a new TransitPath based on an existing Path. The original path
   * instance is not affected in any way.
   *
   * @param engine A reference to the active FPEngine
   * @param sub The SubRoute that will contain this path
   * @param type The type (i.e. transit technology) for this path
   * @param path The Path to serve as the basis for this TransitPath
   * @param addStopAfter Boolean indicating whether to add stops at each node along
   * the path
   */
  public TransitPath(FPEngine engine, SubRoute sub, Type type, Path path, boolean addStop) {
    super(engine);
    drawPathColor_ = Color.GREEN;
    drawStopsColor_ = Color.BLUE;
    drawBasicStops_ = true;
    sub_ = sub;
    stops_ = new ArrayList<Stop>();
    //addedStops_ = new LinkedList<CNode>();
    dirPath_ = new Path(engine);
    type_ = type;

    if (path != null) {
      addStop_ = addStop;
      Iterator<BLink> links = path.getLinks();
      while (links.hasNext()) {
        addLink(links.next());
      }
      this.createDirectPath(engine.getBaseNet());
      //this.updateStopPLIs();
    }

  }

  public SubRoute getSubRoute() {
    return sub_;
  }

  // ROUTINE UPDATE/INIT METHODS
  /*private void updateStopPLIs() {
    Iterator<BLink> links = getLinks();
    Iterator<Stop> stops = stops_.iterator();

    int pli = 0;
    BNode cNode = this.startNode();
    Stop cStop = stops.next();

    // loop covers all but last stop:
    while (links.hasNext()) {
      BLink cLink = links.next();
      System.out.println("--");
      System.out.println("link: "+cLink.getID());
      System.out.println("stop: "+cStop.toString());
      if (cStop.getNode() == cNode) {
        cStop.setPLIs(pli - 1, pli);
        System.out.println("* plis for stop "+cNode.getID()+": "+(pli-1)+" "+(pli));
        cStop = stops.next();
      }
      pli++;
      cNode = (BNode) engine_.getBaseNet().opposite(cNode, cLink);
    }

    // handle last stop separately:
    cStop.setPLIs(pli - 1, -1);
    //System.out.println("* plis for stop "+cStop.getNode().getID()+": "+(pli-1)+" -1");
  }*/

  /**
   * Internal utility method for adjusting the path-link-indices (PLIs)
   * associated with a series of stops within this path up or down by a
   * specified delta. The method requires a "start" PLI; all stops whose
   * out-PLI (i.e. the PLI of the link following the stop) is greater than or
   * equal to the start PLI is adjusted; i.e. all stops adjacent to or
   * following the link specified by the start PLI are adjusted.
   *
   * Typically used when a transit path's internal link structure is modified
   * (e.g. through a split-link or merge-node opertaion affecting the path) in
   * order to "fix" the affected PLIs.
   *
   * @param startAt The PLI at which to start adjusting
   * @param delta The amount by which to adjust the stop PLIs
   */
  private void adjustStopPLIs(int startAt, int delta) {
    System.out.println("adjusting plis for "+sub_.getMasterID());
    int s = 0;
    for(Stop stop : stops_) {
      if(stop.getOutPLI() >= startAt) {
        int newInPLI = stop.getInPLI()+delta;
        int newOutPLI = stop.getOutPLI()+delta;
        System.out.println(" - stop "+s+": ("+stop.getInPLI()+","+stop.getOutPLI()+") to ("+newInPLI+","+newOutPLI+")");
        stop.setPLIs(newInPLI, newOutPLI);
      }
    }
    Stop lastStop = stops_.get(stops_.size()-1);
    lastStop.setPLIs(lastStop.getInPLI()+delta, -1);
  }

  public void createDirectPath(BaseNetwork net) {
    //dirPath_.clear();
    List<BLink> dirPathList = new LinkedList<BLink>();
    //while(!dirPath_.isEmpty()) dirPath_.removeLast();
    for (int i = 0; i < getAllStopCount() - 1; i++) {
      Stop stop1 = stops_.get(i);
      int pli1 = stop1.getOutPLI();
      Stop stop2 = stops_.get(i + 1);
      int pli2 = stop2.getInPLI();
      if (pli1 == pli2) { // no DirectLink needed
        //System.out.println("no d-link needed from "+pli1+" to "+pli2);        
        dirPathList.add(path_.get(pli1));
      } else { // create direct link
        //System.out.println("creating d-link spanning "+pli1+" to "+pli2);
        int dist = 0;
        for (int pli = pli1; pli <= pli2; pli++) {
          BLink link = (BLink) path_.get(pli);
          dist += link.getLengthFeet();
        }
        Scenario s = engine_.getScenarios().getBase(); // temp
        dirPathList.add(net.createNewDLink(s, stop1.getNode(), stop2.getNode(), dist));
      }
    }
    dirPath_.updateList(dirPathList);
  }

  public void registerAllLinks() {
    System.out.println("reg all");
    for (Iterator<BLink> links = getLinks(); links.hasNext();) {
      BLink link = links.next();
      //System.out.println(" link " + link.getID());
      //System.out.println("  before count=" + link.registeredPathCount());
      link.registerPath(this);
      //System.out.println("  after count=" + link.registeredPathCount());
    }
  }

  public void unregisterAllLinks() {
    System.out.println("unreg all");
    for (Iterator<BLink> links = getLinks(); links.hasNext();) {
      BLink link = links.next();
      //System.out.println(" link " + link.getID());
      //System.out.println("  before count=" + link.registeredPathCount());
      link.unregisterPath(this);
      //System.out.println("  after count=" + link.registeredPathCount());
    }
  }

  // ACCESSORS/MUTATORS
  public Path getDirectPath() {
    return dirPath_;
  }

  @Override
  public boolean isTransitPath() {
    return true;
  }

  public void setType(Type type) {
    type_ = type;
  }

  public Type getType() {
    return type_;
  }

  /*public String getTypeName() {
    return getTypeName(type_);
  }

  public static String getTypeName(Type type) {
    switch (type) {
      case METRO: return "Heavy Rail";
      case BUS: return "Local Bus";
      case LIGHTRAIL: return "Light Rail";
      case Type.REGIONALRAIL:
        return "Regional Rail";
    }
    return "unknown";
  }

  public Color getDrawColor() {
    switch (type_) {
      case BUS: return Color.blue;
      case METRO: return Color.red;
      case REGIONALRAIL: return Color.magenta;
      case LIGHTRAIL: return Color.orange;
    }
    return Color.gray;
  }*/

  public void setDrawColors(Color pathColor, Color stopsColor) {
    drawPathColor_ = pathColor;
    drawStopsColor_ = stopsColor;
  }

  public void setDrawBasicStops(boolean val) {
    drawBasicStops_ = val;
  }

  @Override
  public void draw(MapCanvas canvas) { //, Graph graph) {

    if (isEmpty()) {
      return;
    }

    //super.setDefaultDrawColor(drawPathColor_);
    //super.draw(canvas, true);
    Color bgColor = new Color(0, 127, 0), fgColor = drawPathColor_;

    super.draw(canvas, Color.white, 10);

    drawStops(canvas, Color.white, 9, false);
    drawStops(canvas, bgColor, 7, false);

    super.draw(canvas, bgColor, 6);
    super.drawArrows(canvas, fgColor, 1, bgColor);
    super.draw(canvas, fgColor, 3);

    drawStops(canvas, fgColor, 3, true);

  }

  private void drawStops(MapCanvas canvas, Color color, int width, boolean annotate) {

    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop stop = stops.next();
      if (!stop.isTimePoint() && !drawBasicStops_) {
        continue;
      }
      canvas.setColor(color);
      stop.draw(canvas, width, annotate);
    }

  }

  public Collection<Stop> getStops() {
    return stops_;
  }

  public Iterator<Stop> getStopIter() {
    return stops_.iterator();
  }

  public Iterator<TimePoint> getTimePoints() {
    Iterator<Stop> stops = stops_.iterator();
    List<TimePoint> tpts = new LinkedList<TimePoint>();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint()) {
        tpts.add((TimePoint) s);
      }
    }
    return tpts.iterator();
  }

  public Iterator<TimePoint> getTimePoints(BNode node) {
    List<TimePoint> list = new LinkedList<TimePoint>();
    for (int pli = -1; pli < path_.size(); pli++) {
      if (toNode(pli) == node && timePointFollows(pli)) {
        list.add((TimePoint) getStop(this.getStopIndexFromInPLI(pli)));
      }
    }
    return list.iterator();
  }

  public Stop getStop(int index) {
    //System.out.println("i: "+index);
    return stops_.get(index);
  }

  public List<Stop> getStopList() {
    return stops_;
  }

  public void setStopList(List<Stop> stops) {
    stops_ = stops;
  }

  public int getStopIndex(BNode node) {
    Iterator<Stop> stops = stops_.iterator();
    int i = 0;
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.getNode() == node) {
        return i;
      }
      i++;
    }
    return -1;
  }

  /**
   * Returns a numeric index of a stop (where 0 is the starting timepoint) 
   * given the "In-PLI" (the path link index of the link that immediately
   * precedes the stop node along the path). 
   * 
   * @param inPLI the path link index of the link preceding the stop
   * @return the stop index, or -1 if no stop exists
   */
  public int getStopIndexFromInPLI(int inPLI) {
    Iterator<Stop> stops = stops_.iterator();
    int i = 0;
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.getInPLI() == inPLI) {
        return i;
      }
      i++;
    }
    return -1;
  }

  public int getAllStopCount() {
    return stops_.size();
  }

  public int getMasterIndex(int tptIndex) {
    Iterator<Stop> stops = stops_.iterator();
    int i = 0;
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint() && ((TimePoint) s).getIndex() == tptIndex) {
        return i;
      }
      i++;
    }
    return -1;
  }

  public TimePoint getTimePoint(int tptIndex) {
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint() && ((TimePoint) s).getIndex() == tptIndex) {
        return (TimePoint) s;
      }
    }
    return null;
  }

  public int getTimePointCount() {
    int count = 0;
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint()) {
        count++;
      }
    }
    return count;
  }

  public TimePoint getFirstTimePoint() {
    return (TimePoint) stops_.get(0);
  }

  public TimePoint getLastTimePoint() {
    return (TimePoint) stops_.get(stops_.size() - 1);
  }

  public int getFirstPLIFromDLI(int dli) {
    return getStop(dli).getOutPLI();
  }

  public int getLastPLIFromDLI(int dli) {
    return getStop(dli + 1).getInPLI();
  }

  // LINK ADDITION/REMOVAL METHODS
  public void initLink(BLink link) {
    path_.add(link);
    if (path_.size() == 1) {
      firstFNode_ = link.getFNode();
    } else if (path_.size() == 2 && (firstFNode_ == link.getFNode() || firstFNode_ == link.getTNode())) {
      firstFNode_ = engine_.getBaseNet().opposite(firstFNode_, path_.get(0));
    }
    link.registerPath(this);
  }

  public boolean checkScenario(Scenario s) {
    //System.out.println("cehcking scenario "+s.getName()+ " against "+sub_.getRoute().getScenario().getName());
    return s == sub_.getRoute().getScenario() || s.isAncestorOf(sub_.getRoute().getScenario());
  }

  @Override
  protected void addFirst(BLink link) {
    if (!link.supportsTransitType(type_, sub_.getRoute().getScenario(), baseNet_)) {
      if (guiOA_ != null) {
        guiOA_.msg("f Transit type not supported by link (f)");
      }
      return;
    }
    if (!checkScenario(link.getScenario())) {
      if (guiOA_ != null) {
        guiOA_.msg("Active scenario not compatible with link " + link.getID());
      }
      return;
    }
    super.addFirst(link);

    // initialize the start and end timepoints:
    stops_.add(stops_.size(), new TimePoint(this, firstFNode_, 1, -1, 0));
    stops_.add(stops_.size(), new TimePoint(this, baseNet_.opposite(firstFNode_, link), 2, 0, -1));
  }

  @Override
  public boolean addLink(BLink link) {

    if (!link.supportsTransitType(type_, sub_.getRoute().getScenario(), baseNet_)) {
      if (guiOA_ != null) {
        guiOA_.msg("Transit type not supported by link (a)");
      }
      return false;
    }
    if (!checkScenario(link.getScenario())) {
      if (guiOA_ != null) {
        guiOA_.msg("Active scenario not compatible with link " + link.getID());
      }
      return false;
    }

    //addedStops_.clear();
    //while(!addedStops_.isEmpty()) addedStops_.removeLast();
    return super.addLink(link);
  }

  @Override
  public boolean addToStart(BLink link) {
    if (!link.supportsTransitType(type_,  sub_.getRoute().getScenario(), baseNet_)) {
      if (guiOA_ != null) {
        guiOA_.msg("Transit type not supported by link (s)");
      }
      return false;
    }
    if (!checkScenario(link.getScenario())) {
      if (guiOA_ != null) {
        guiOA_.msg("Active scenario not compatible with link " + link.getID());
      }
      return false;
    }

    if (path_.size() == 1 && baseNet_.areIncident(getFirstTimePoint().getNode(), link)) {
      /*stops_.swapElements(stops_.first(), stops_.last());
      getFirstTimePoint().setIndex(1);
      getLastTimePoint().setIndex(2);
      getLastTimePoint().setPLIs(1, -1);*/

      System.out.println("swapping");
      firstFNode_ = engine_.getBaseNet().opposite(firstFNode_, path_.get(0));

      // swap elements
      //stops_.swapElements(stops_.get(0), stops_.get(stops_.size()-1));
      List<Stop> swapped = new ArrayList<Stop>();
      swapped.add(stops_.get(1));
      swapped.add(stops_.get(0));
      stops_ = swapped;

      getLastTimePoint().setIndex(2);
      getLastTimePoint().setPLIs(1, -1);
      getFirstTimePoint().setIndex(1);
      getFirstTimePoint().setPLIs(-1, 0);
      return addToEnd(link);

    }

    super.addToStart(link);

    // move first timepoint
    //Vertex[] endPoints = engine_.getGraph().endVertices(link.getEdge()); 
    BNode fNode = link.getFNode();
    BNode tNode = link.getTNode();

    BLink nextLink = getLink(1); //(CLink) path_.after(path_.first()).element();
    BNode stopNode;
    if (baseNet_.areIncident(fNode, nextLink)) {
      getFirstTimePoint().setNode(tNode);
      newFirstLinkPLIs(1);
      stopNode = fNode;
    } else {
      getFirstTimePoint().setNode(fNode);
      newFirstLinkPLIs(1);
      stopNode = tNode;
    }

    if (addStop_ && engine_.getBaseNet().walkableLinkCount(stopNode) > 2) {
      System.out.println("adding stop from addToStart");
      addStopAfter(0, false);
    //addedStops_.add(stopNode);
    }
    //int firstPLI = getFirstPLI((CLink) path_.first().element());
    //gui_.msg("fpli: "+firstPLI);
    //getFirstTimePoint().setPLIs(-1, 0);
    return true;
  }

  private void newFirstLinkPLIs(int delta) {
    Iterator<Stop> stops = stops_.iterator();
    int i = 0;
    while (stops.hasNext()) {
      Stop stop = stops.next();
      if (i != 0 && i < stops_.size() - 1) {
        stop.setPLIs(stop.getInPLI() + delta, stop.getOutPLI() + delta);
      } else if (i == stops_.size() - 1) {
        stop.setPLIs(stop.getInPLI() + delta, -1);
      }
      i++;
    }
  }

  @Override
  public boolean addToEnd(BLink link) {
    if (!link.supportsTransitType(type_, sub_.getRoute().getScenario(), baseNet_)) {
      if (guiOA_ != null) {
        guiOA_.msg("Transit type not supported by link (e)");
      }
      return false;
    }
    if (!checkScenario(link.getScenario())) {
      if (guiOA_ != null) {
        guiOA_.msg("Active scenario not compatible with link " + link.getID());
      }
      return false;
    }

    super.addToEnd(link);

    if (path_.size() == 2 && !baseNet_.areIncident(getLastTimePoint().getNode(), link)) {
      // swap elements
      List<Stop> swapped = new ArrayList<Stop>();
      swapped.add(stops_.get(1));
      swapped.add(stops_.get(0));
      stops_ = swapped;
      getFirstTimePoint().setIndex(1);
      getFirstTimePoint().setPLIs(-1, 0);
      getLastTimePoint().setIndex(2);
      System.out.println("swapping");
    }

    // move last timepoint
    //Vertex[] endPoints = engine_.getGraph().endVertices(link.getEdge()); 
    BNode fNode = link.getFNode();
    BNode tNode = link.getTNode();


    BLink previousLink = path_.get(path_.size() - 2); //(CLink) path_.before(path_.last()).element();
    int inPLI = path_.size() - 2; //getFirstPLI(previousLink);
    if (inPLI < -1) {
      inPLI = -1;
    }
    BNode stopNode;
    if (baseNet_.areIncident(fNode, previousLink)) {
      //stops_.replaceElement(stops_.last(), new CTimePoint((CNode) endPoints[1].element()));
      getLastTimePoint().setNode(tNode);
      stopNode = fNode;
    } else {
      //stops_.replaceElement(stops_.last(), new CTimePoint((CNode) endPoints[0].element()));
      getLastTimePoint().setNode(fNode);
      stopNode = tNode;
    }

    if (addStop_ && engine_.getBaseNet().walkableLinkCount(stopNode) > 2) {
      addStopAfter(inPLI, false);
    //addedStops_.add(stopNode);
    }

    int lastPLI = path_.size() - 1; //getFirstPLI((CLink) path_.last().element());
    //if(guiOA_ != null) guiOA_.msg("lpli: "+lastPLI);
    getLastTimePoint().setPLIs(lastPLI, -1);
    return true;

  }
  //public Iterator<CNode> getAddedStops() { return addedStops_.iterator(); }
  public static final int REM_ERR_INLINE_STOP = 10;

  @Override
  public int removeLastLink() {
    BNode opp = baseNet_.opposite(getLastTimePoint().getNode(), endLink());

    if (stopPrecedes(linkCount()-1)) {
      if (guiOA_ != null) {
        guiOA_.msg("Remove inline stop first");
      }
      return REM_ERR_INLINE_STOP;
    }
    //Position bstop = getBasicStopPos(opp);
    //if(bstop != null) stops_.remove(bstop);
    stops_.remove(getBasicStopAtNode(opp));

    if (path_.size() == 2) {
      super.removeLastLink();
      //stops_.replaceElement(stops_.last(), graph_.opposite(((CNode) stops_.first().element()).getVertex(), ((CLink) path_.first().element()).getEdge()).element());
      getLastTimePoint().setNode(baseNet_.opposite(getFirstTimePoint().getNode(), startLink()));
      getLastTimePoint().setPLIs(0, -1);
    } else {
      BLink lastLink = endLink();
      BLink previousLink = getLink(path_.size() - 2); //(CLink) path_.before(path_.last()).element();

      //Vertex[] endPoints = engine_.getGraph().endVertices(previousLink.getEdge());
      BNode fNode = previousLink.getFNode();
      BNode tNode = previousLink.getTNode();

      if (baseNet_.areIncident(fNode, lastLink)) {
        //stops_.replaceElement(stops_.last(), new CTimePoint((CNode) endPoints[0].element()));
        getLastTimePoint().setNode(fNode);
      } else {
        //stops_.replaceElement(stops_.last(), new CTimePoint((CNode) endPoints[1].element()));
        getLastTimePoint().setNode(tNode);
      }
      getLastTimePoint().setPLIs(path_.size() - 2, -1); //getFirstPLI(previousLink), -1);
      //path_.removeLast();		
      super.removeLastLink();
    }
    return 0;
  }

  @Override
  public int removeFirstLink() {
    BNode opp = baseNet_.opposite(getFirstTimePoint().getNode(), startLink());

    if (stopFollows(0)) {
      if (guiOA_ != null) {
        guiOA_.msg("Remove inline stop first");
      }
      return REM_ERR_INLINE_STOP;
    }
    //Position bstop = getBasicStopPos(opp);
    //if(bstop != null) stops_.remove(bstop);
    stops_.remove(getBasicStopAtNode(opp));

    if (path_.size() == 2) {
      super.removeFirstLink();
      //stops_.replaceElement(stops_.first(), graph_.opposite(((CNode) stops_.last().element()).getVertex(), ((CLink) path_.last().element()).getEdge()).element());
      getFirstTimePoint().setNode(baseNet_.opposite(getLastTimePoint().getNode(), endLink()));
      getFirstTimePoint().setPLIs(-1, 0);
    } else {
      BLink firstLink = startLink();
      BLink nextLink = getLink(1); //(CLink) path_.after(path_.first()).element();

      //Vertex[] endPoints = engine_.getGraph().endVertices(nextLink.getEdge());
      BNode fNode = nextLink.getFNode();
      BNode tNode = nextLink.getTNode();

      if (baseNet_.areIncident(fNode, firstLink)) {
        //stops_.replaceElement(stops_.first(), new CTimePoint((CNode) endPoints[0].element()));
        getFirstTimePoint().setNode(fNode);
      } else {
        //stops_.replaceElement(stops_.first(), new CTimePoint((CNode) endPoints[1].element()));
        getFirstTimePoint().setNode(tNode);
      }
      //path_.removeFirst();
      super.removeFirstLink();
      getFirstTimePoint().setPLIs(-1, 0); //getFirstPLI(nextLink));
      newFirstLinkPLIs(-1);
    }
    return 0;
  }

  @Override
  public void clear() {
    super.clear();
    while (!stops_.isEmpty()) {
      stops_.remove(stops_.size() - 1);
    }
  }

  // STOP-RELATED METHODS
  public void initStop(Stop stop) {
    stops_.add(stops_.size(), stop);
  }

  public void initTimePoint(int nodeID, int i, int inPLI, int outPLI) {
    stops_.add(stops_.size(), new TimePoint(this, nodeID, i, inPLI, outPLI));
  }

  public void initBasicStop(int nodeID, int inPLI, int outPLI) {
    stops_.add(stops_.size(), new BasicStop(this, nodeID, inPLI, outPLI));
  }


  /*public void initTimePoint(BNode node, int i, int inPLI, int outPLI) {
    stops_.add(stops_.size(), new TimePoint(this, node, i, inPLI, outPLI));
  }

  public void initBasicStop(BNode node, int inPLI, int outPLI) {
    stops_.add(stops_.size(), new BasicStop(this, node, inPLI, outPLI));
  }*/

  public boolean addStopAfter(int inPLI, boolean isTimePoint) {
    //System.out.println("tpath addStop");
    BNode node = this.toNode(inPLI);
    Iterator<BLink> links = getLinks();
    int incLinks = 0;
    BLink link;
    while (links.hasNext()) {
      link = links.next();
      if (baseNet_.areIncident(node, link)) {
        incLinks++;
      }
    }
    if (incLinks == 0) {
      if (guiOA_ != null) {
        guiOA_.msg("Not on path");
      }
      return false;
    } else if (incLinks == 1) {
      if (guiOA_ != null) {
        guiOA_.msg("Endpoint selected");
      }
      return false;
    } else {
      //System.out.println("***** incLinks: " + incLinks);
      BNode curNode = getFirstTimePoint().getNode();
      links = getLinks();
      Iterator<Stop> stops = new ArrayList(stops_).iterator();
      Stop curStop = stops.next();
      int curStopIndex = 0, addedStopIndex = -1, stopsPassed = 0, timePtsPassed = 0, nodesPassed = 0;
      boolean addedTimePt = false;
      int curOutPLI = 0, curInPLI = -1;
      while (nodesPassed < path_.size()) {

        //CStop curStop = (CStop) curStopPos.element();
        link = links.next();
        if (curNode == curStop.getNode() && curStop.getInPLI() == curInPLI) {
          stopsPassed++;
          if (curStop.isTimePoint()) {
            //if(guiOA_ != null) guiOA_.msg("time pts passed: "+timePtsPassed);
            timePtsPassed++;
            if (addedTimePt) {
              ((TimePoint) curStop).setIndex(((TimePoint) curStop).getIndex() + 1);
            }
          }
          curStop = stops.next();
          curStopIndex++;
        }
        if (curNode == node && curInPLI == inPLI) { // condition for adding the stop 
          //System.out.println("ADDED time pts passed: " + timePtsPassed);
          if (isTimePoint) { // add a new timepoint
            stops_.add(curStopIndex, new TimePoint(this, node, timePtsPassed + 1, curInPLI, curOutPLI));
            addedTimePt = true;
            //sub_.addStopAfter(curStopIndex);
            addedStopIndex = curStopIndex;
          } else { // add basic stop
            //if(guiOA_ != null) guiOA_.msg("adding basic stop ("+curInPLI+","+curOutPLI+")");
            stops_.add(curStopIndex, new BasicStop(this, node, curInPLI, curOutPLI));
            //sub_.addStopAfter(curStopIndex);
            addedStopIndex = curStopIndex;
            break;
          }


        //break;
        }
        curNode = baseNet_.opposite(curNode, link);
        curOutPLI++;
        curInPLI++;
        nodesPassed++;
      }
      if (addedTimePt) {
        getLastTimePoint().setIndex(getLastTimePoint().getIndex() + 1);
      }
      if (addedStopIndex >= 0) {
        sub_.addStopToTimetables(addedStopIndex);
      }
    }
    return true;
  }

  public boolean deleteStop(int stopIndex) {
    Iterator<Stop> stops = stops_.iterator();
    boolean deletedTimePt = false;
    int i = 0;
    Stop toRemove = null;
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (deletedTimePt && s.isTimePoint()) {
        ((TimePoint) s).setIndex(((TimePoint) s).getIndex() - 1);
      }
      if (!s.isTimePoint() && i == stopIndex) {
        toRemove = s;
        break;
      } else if (s.isTimePoint() && i == stopIndex) {
        toRemove = s;
        deletedTimePt = true;
      }
      i++;
    }
    if (toRemove != null) {
      stops_.remove(toRemove);
      sub_.removeStopFromTimetables(stopIndex);
      return true;
    }
    return false;
  }

  public boolean stopFollows(int pli) {
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.getInPLI() == pli) {
        return true;
      }
    }
    return false;
  }

  public boolean basicStopFollows(int pli) {
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (!s.isTimePoint() && s.getInPLI() == pli) {
        return true;
      }
    }
    return false;
  }

  public boolean timePointFollows(int pli) {
    if (pli == -1) {
      return true;
    }
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint() && s.getInPLI() == pli) {
        return true;
      }
    }
    return false;
  }

  public boolean stopPrecedes(int pli) {
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.getOutPLI() == pli) {
        return true;
      }
    }
    return false;
  }

  public int getStopCountAtNode(BNode node) {
    int count = 0;
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.getNode() == node) {
        count++;
      }
    }
    return count;
  }

  public int getBasicStopCountAtNode(BNode node) {
    int count = 0;
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (!s.isTimePoint() && s.getNode() == node) {
        count++;
      }
    }
    return count;
  }

  public int getTimePointCountAtNode(BNode node) {
    int count = 0;
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint() && s.getNode() == node) {
        count++;
      }
    }
    return count;
  }

  public Iterator<Integer> getAvailableStopInPLIs(BNode node) {
    List<Integer> list = new LinkedList<Integer>();
    for (int pli = 0; pli < path_.size(); pli++) {
      if (toNode(pli) == node && !stopFollows(pli)) {
        list.add(new Integer(pli));
      }
    }
    return list.iterator();
  }

  public Iterator<Integer> getBasicStopInPLIs(BNode node) {
    List<Integer> list = new LinkedList<Integer>();
    for (int pli = 0; pli < path_.size(); pli++) {
      if (toNode(pli) == node && basicStopFollows(pli)) {
        list.add(new Integer(pli));
      }
    }
    return list.iterator();
  }

  public Iterator<Integer> getTimePointInPLIs(BNode node) {
    List<Integer> list = new LinkedList<Integer>();
    for (int pli = 0; pli < path_.size(); pli++) {
      if (toNode(pli) == node && timePointFollows(pli)) {
        list.add(new Integer(pli));
      }
    }
    return list.iterator();
  }

  public boolean hasStop(BNode node) {
    //return hasStop(node.getVertex()); 
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.getNode() == node) {
        return true;
      }
    }
    return false;
  }

  public boolean hasTimePoint(BNode node) {
    //return hasTimePoint(node.getVertex()); 
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (s.isTimePoint() && s.getNode() == node) {
        return true;
      }
    }
    return false;
  }

  public boolean hasBasicStop(BNode node) {
    //return hasBasicStop(node.getVertex()); 
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (!s.isTimePoint() && s.getNode() == node) {
        return true;
      }
    }
    return false;
  }

  public Stop getBasicStopAtNode(BNode node) {
    Iterator<Stop> stops = stops_.iterator();
    while (stops.hasNext()) {
      Stop s = stops.next();
      if (!s.isTimePoint() && s.getNode() == node) {
        return s;
      }
    }
    return null;
  }

  public int getDistanceBetweenStops(int i1, int i2, boolean measured) {
    int dist = 0;
    //System.out.println("stop range: "+i1+" to "+i2);
    int pli1 = getStop(i1).getOutPLI();
    int pli2 = getStop(i2).getInPLI();
    //System.out.println("pli range: "+pli1+" to "+pli2);
    for (int pli = pli1; pli <= pli2; pli++) {
      dist += measured ? getLink(pli).getMeasuredLength() : getLink(pli).getLengthFeet();
    }
    return dist;
  }

  public void setAddStop(boolean addStop) {
    addStop_ = addStop;
  }

  // SPECIAL EDITING METHODS
  public void mergeNode(BNode node1, BLink link, BNode node2) {
    // remove deleted link from path:
		/*PositionIterator linkPosIter = path_.positions();
    while(linkPosIter.hasNext()) {
    Position linkPos = linkPosIter.nextPosition();
    if(linkPos.element() == link) {
    guiOA_.msg("deleting link pos "+link.getID());
    guiOA_.msg("len before "+path_.size());
    path_.remove(linkPos);
    guiOA_.msg("len after "+path_.size());
    }
    }*/
    while (path_.contains(link)) {
      int pli = path_.indexOf(link);
      path_.remove(link);
      link.unregisterPath(this);
      this.adjustStopPLIs(pli, -1);
    }

    //updateStopPLIs();
  }

  public void splitLink(BLink delLink, BLink fLink, BLink tLink) {
    //PositionIterator linkPosIter = path_.positions();
    while (path_.contains(delLink)) {
      splitLinkInstance(delLink, fLink, tLink);
    }
  }

  private void splitLinkInstance(BLink delLink, BLink fLink, BLink tLink) {
    BNode fNode = startNode();
    //System.out.println("delLink fn " + delLink.getFNodeID() + " tn " + delLink.getTNodeID());
    //while(linkPosIter.hasNext()) {
    for (int pli = 0; pli < path_.size(); pli++) {
      //Position linkPos = linkPosIter.nextPosition();
      BLink link = path_.get(pli); //(CLink) linkPos.element(); 
      if (link == delLink) {
        guiOA_.msg("deleting link " + delLink.getID());
        //guiOA_.msg("len before "+path_.size());
        //System.out.println("link fn " + link.getFNodeID() + " tn " + link.getTNodeID());
        if (fNode == delLink.getFNode()) {
          //path_.insertBefore(linkPos, fLink);
          //path_.insertAfter(linkPos, tLink);
          path_.add(pli, tLink);
          path_.add(pli, fLink);
        } else if (fNode == delLink.getTNode()) {
          //path_.insertBefore(linkPos, tLink);
          //path_.insertAfter(linkPos, fLink);					
          path_.add(pli, fLink);
          path_.add(pli, tLink);
        } else {
          System.out.println("error in splitLink: node does not match link");
        }
        path_.remove(link);
        this.adjustStopPLIs(pli+1, 1);
        //guiOA_.msg("len after "+path_.size());
        break;
      }
      fNode = engine_.getBaseNet().opposite(fNode, link);
    }
    //updateStopPLIs();
  }

  public void stitchLinks(BLink link1, BLink link2, BLink newLink) {
    System.out.println("Stitching links for "+sub_.getMasterID());
    for(int pli=0; pli<path_.size()-1; pli++) {
      if((path_.get(pli)==link1 && path_.get(pli+1)==link2) ||
              (path_.get(pli)==link2 && path_.get(pli+1)==link1)) {
        path_.remove(pli);
        path_.remove(pli);
        path_.add(pli, newLink);
        adjustStopPLIs(pli+2, -1);
      }
    }

    //updateStopPLIs();
  }

  public TransitPath createCopy(FPEngine engine, boolean reverse) {
    return createCopy(engine, sub_, reverse);
  }

  public TransitPath createCopy(FPEngine engine, SubRoute newParent, boolean reverse) {
    TransitPath copy = new TransitPath(engine, newParent, type_);

    // copy links
    Iterator<BLink> links = getLinks();
    while (links.hasNext()) {
      if (reverse) {
        copy.path_.add(0, links.next());
      } else {
        copy.path_.add(links.next());
      }
    }

    copy.firstFNode_ = reverse ? endNode() : startNode();

    // copy stops
    Iterator<Stop> stops = stops_.iterator();
    int numTPts = this.getTimePointCount();
    int maxPLI = linkCount() - 1;
    while (stops.hasNext()) {
      Stop stop = (stops.next()).createCopy(copy);
      if (reverse) {
        int newOutPLI = stop.getInPLI() == -1 ? -1 : maxPLI - stop.getInPLI();
        int newInPLI = stop.getOutPLI() == -1 ? -1 : maxPLI - stop.getOutPLI();
        System.out.println("inpli from " + stop.getInPLI() + " to " + newInPLI);
        System.out.println("outpli from " + stop.getOutPLI() + " to " + newOutPLI);
        System.out.println(" ");
        stop.setPLIs(newInPLI, newOutPLI);
        if (stop.isTimePoint()) {
          ((TimePoint) stop).setIndex(numTPts - ((TimePoint) stop).getIndex() + 1);
        }
        copy.stops_.add(0, stop);
      } else {
        copy.stops_.add(stop);
      }
    }

    //copy.updateStopPLIs();
    if(dirPath_ != null && dirPath_.linkCount() >0) copy.createDirectPath(engine.getBaseNet());

    return copy;
  }

  public boolean divertPath(Path diversion) {
    if (!this.visitsNode(diversion.startNode()) || !this.visitsNode(diversion.startNode())) {
      return false;
    }
    Stop lastStop;
    int lastStopIndex = 0;
    boolean removing = false;
    BNode lastStopNode = startNode();
    int numRemoved = 0, pli = 0, lastPLI = -1;
    BNode toNode = startNode();

    // Step 1: remove links being cut out of path
    // (not necessary if diversion path begins and ends at same node)

    //System.out.println("STOP COUNT BEFORE = "+this.getStopList().size());
    Iterator<BLink> links = this.createCopy(engine_, sub_, false).getLinks();
    while (links.hasNext()) {
      BLink link = links.next();
      //System.out.println("pli "+pli+" link "+link.getID()+" tonode "+toNode.getID());

      toNode = engine_.getBaseNet().opposite(toNode, link);
      if (removing) {
        if (toNode != diversion.endNode() && hasStop(toNode)) {
          deleteStop(getStopIndexFromInPLI(pli));
        }
        path_.remove(link);
        //System.out.println("removing link "+link.getID());
        numRemoved++;
      } else {
        if (hasStop(toNode)) {
          lastStopNode = toNode;
        }
      }
      if (toNode == diversion.startNode()) {
        removing = true;
        lastPLI = pli;
      }
      if (toNode == diversion.endNode()) {
        break;
      }
      pli++;
    } // end of link removal loop
    System.out.println("lastPLI="+lastPLI);

    //System.out.println("STOP COUNT MID = "+this.getStopList().size());

    /** Step 2: insert diversion links to appropriate point within path **/
    if (lastPLI != -1) { // skip step if removal check failed 

      // adjust the stop PLIs affected by the diversion

      int startAt = lastPLI+numRemoved+1;
      int delta = diversion.linkCount()-numRemoved;
      System.out.println("starting at "+startAt+", delta="+delta);
      this.adjustStopPLIs(startAt, delta);

      // find the actual last stop corresponding to lastStopNode 
      Iterator<Stop> stops = stops_.iterator();
      int i = 0;
      while (stops.hasNext()) {
        Stop s = stops.next();
        if (s.getNode() == lastStopNode) {
          lastStop = s;
          lastStopIndex = i;
        }
        i++;
      }

      // special stop addition case for diversions that begin/end at same node
      if (addStop_ && numRemoved == 0) {
        Stop stop = new BasicStop(this, diversion.startNode(), lastPLI, lastPLI+1);
        lastStopIndex++;
        stops_.add(lastStopIndex, stop);
        sub_.addStopToTimetables(lastStopIndex, false);
      }
      
      // add the diversion links
      Iterator<BLink> divLinks = diversion.getLinks();
      i = 0;
      pli = lastPLI;
      while (divLinks.hasNext()) {
        pli++;
        BLink link = divLinks.next();
        path_.add(pli, link);
        System.out.println("added link " + link.getID());
        BNode stopNode = fromNode(pli);
        System.out.println("i="+1+" addStop_="+addStop_);
        if (i > 0 && addStop_ && engine_.getBaseNet().walkableLinkCount(stopNode) > 2) {
          System.out.println("new stop at "+stopNode.getID());
          lastStop = new BasicStop(this, stopNode, pli-1, pli);
          lastStopIndex++;
          stops_.add(lastStopIndex, lastStop);
          sub_.addStopToTimetables(lastStopIndex, false);
        }
        //System.out.println("adding link "+link.getID());
        i++;
      }


    }

    //System.out.println("STOP COUNT AFTER = "+this.getStopList().size());


    sub_.initTimePointIndices();

    
    sub_.calculateBasicStopTimes();
    createDirectPath(engine_.getBaseNet());
    return true;
  }

  /**
   * Clips a "circuit" diversion, a route feature where a detour both leaves and
   * rejoins the main path at a common node. The circuit is removed in full, and
   * the main "trunk" path is unaffected.
   *  
   * @param node the node at which the circuit leaves and rejoins the path
   * @param net the network
   * @param out an output acceptor for error / status messages
   */
  public void clipCircuitDiversion(BNode node, BaseNetwork net, GUIOutputAcceptor out) {
    if (hasStop(node)) {
      out.msg("Remove stops/timepoints first");
      return;
    }
    Iterator<BLink> iLinks = net.incidentLinks(node).iterator();
    int order1 = 0, order2 = 0, higher = 0;
    BLink o2link = null;
    while (iLinks.hasNext()) {
      BLink link = iLinks.next();
      int order = countLinkOccurences(link);
      if (order == 1) {
        order1++;
      } else if (order == 2) {
        o2link = link;
        order2++;
      } else if (order > 2) {
        higher++;
      }
    }
    //out.msg("ocount: "+order1+" "+order2+" "+higher);
    if (order1 != 2 || order2 != 1 || higher != 0) {
      out.msg("No diversion found at node");
      return;
    }

    Iterator<BLink> links = getLinks();
    List<BLink> remLinks = new LinkedList<BLink>();
    BLink link = null;
    int pli = 0;//, startPLI = 0;
    while (links.hasNext()) { // find first link and remove it
      link = links.next();
      if (link == o2link) {
        if (this.stopFollows(pli)) {
          deleteStop(getStopIndexFromInPLI(pli));
        }
        //path_.remove(link);
        remLinks.add(link);
        break;
      }
      pli++;
    }

    link = links.next();
    pli++;
    while (link != o2link) { // remove interceding links 
      //path_.remove(link);
      remLinks.add(link);
      if (stopFollows(pli)) {
        deleteStop(getStopIndexFromInPLI(pli));
      }
      link = links.next();
      pli++;
    }

    // remove last link (2nd occurence of o2link)
    remLinks.add(link);
    //path_.remove(link);

    Iterator<BLink> rlIter = remLinks.iterator();
    while (rlIter.hasNext()) {
      path_.remove(rlIter.next());
    }
    this.adjustStopPLIs(pli+1, -remLinks.size());
    //updateStopPLIs();
    createDirectPath(net);

  }


  // importPathFromText //
  private List<BLink> iPfT_links_;
  private BNode iPfT_origin_;
  private BLink iPfT_curLink_;
  private boolean iPfT_debug_;

  public boolean importPathFromText(BNode first, BNode second, String text, boolean addStop) {
    iPfT_debug_ = true;
    addStop_ = addStop;
    iPfT_links_ = new LinkedList<BLink>(); //NodeSequence();
    iPfT_origin_ = first;
    iPfT_curLink_ = baseNet_.connectingLinks(first, second).iterator().next();
    try {
      BufferedReader reader = new BufferedReader(new StringReader(text));
      boolean success = true;
      int lineNum = 1;
      while (reader.ready() && success) {
        String line = reader.readLine();
        if (line == null) {
          break;
        }
        if (iPfT_debug_) {
          System.out.println("l: " + line);
        }
        if (line.contains("onto")) {
          success = extendRoute(line);
          if (!success) {
            if (iPfT_debug_) {
              System.out.println(" ** failed **");
            }
            guiOA_.msg("Failed on following line:");
            guiOA_.msg("(" + lineNum + ") \"" + line + "\"");
          }
        }
        lineNum++;
      }
      reader.close();
      if (iPfT_debug_) {
        System.out.println(" potential path length: " + iPfT_links_.size());
      }
      if (success) {
        iPfT_links_.add(iPfT_curLink_);
        Iterator<BLink> addLinks = iPfT_links_.iterator();
        guiOA_.msg("Successfully processed text directions");
        while (addLinks.hasNext()) {
          BLink addLink = addLinks.next();
          if (iPfT_debug_) {
            System.out.println("# adding link to path: " + addLink.getID());
          }
          addLink(addLink);
        }
        return true;
      }
    } catch (IOException e) {
      e.printStackTrace();
    }
    return false;
  }

  // helper methods for importPathFromText
  private String simplifyStreetName(String stName) {
    stName = FPUtil.standardizeStreetName(stName.trim());
    StreetNameParser splitter = new StreetNameParser(stName, engine_.getLocations(), false);
    return (splitter.getResolved().getName() + splitter.getResolved().getFacType()).trim();
  }

  private boolean extendRoute(String nextInstruction) {
    String curStName = simplifyStreetName(iPfT_curLink_.getDisplayName());
    if (iPfT_debug_) {
      System.out.println(" cur street: " + curStName);
    }

    String[] tokens = nextInstruction.split("onto");
    String nextDir = tokens[0].trim().toLowerCase(), nextStName = simplifyStreetName(tokens[1]);
    if (nextDir.contains("left")) {
      nextDir = "left";
    }
    if (nextDir.contains("right")) {
      nextDir = "right";
    }
    if (iPfT_debug_) {
      System.out.println(" next dir: " + nextDir);
    }
    if (iPfT_debug_) {
      System.out.println(" next street: " + nextStName);
    }

    BNode curOrigin = iPfT_origin_;
    BLink curLink = iPfT_curLink_;
    List<BLink> newLinks = new LinkedList<BLink>();

    int limit = 50, i = 0;

    List<BLink> nextStreetLinks = new LinkedList<BLink>();
    BNode toNode = null;

    while (i < limit) {
      toNode = baseNet_.opposite(curOrigin, curLink);
      Iterator<BLink> links = baseNet_.incidentLinks(toNode).iterator();
      BLink thisStreetNextLink = null;
      while (links.hasNext()) {
        BLink link = links.next();
        if (link != curLink) {
          String eStName = simplifyStreetName(link.getDisplayName());
          if (eStName.compareTo(curStName) == 0) {
            thisStreetNextLink = link;
          }
          if (eStName.compareTo(nextStName) == 0) {
            nextStreetLinks.add(link);
          }
        }
      }
      if (nextStreetLinks.size() > 0) {
        if (nextStreetLinks.size() == 1) { // check that this is correct turn
          double det = calcDet(curLink, nextStreetLinks.get(0));
          if (iPfT_debug_) {
            System.out.println(" checking for correct turn");
          }
          if (!nextDir.equals("left") && !nextDir.equals("right")) {
            if (iPfT_debug_) {
              System.out.println(" breaking; dir is not left or right");
            }
            break;
          }
          if (nextDir.equals("left") && det > 0) {
            if (iPfT_debug_) {
              System.out.println(" breaking; valid left turn");
            }
            break;
          }
          if (nextDir.equals("right") && det < 0) {
            if (iPfT_debug_) {
              System.out.println(" breaking; valid right turn");
            }
            break;
          }
          if (iPfT_debug_) {
            System.out.println(" bypassing potential turn at link " + nextStreetLinks.get(0).getID());
          }
          nextStreetLinks.remove(0);
        } else {
          break;
        }
      }
      if (thisStreetNextLink == null) {
        if (iPfT_debug_) {
          System.out.println(" reached dead end");
        }
        return false;
      }
      newLinks.add(curLink);
      curOrigin = toNode;
      curLink = thisStreetNextLink;
      i++;
    }

    if (nextStreetLinks.size() > 0) {
      newLinks.add(curLink);
      if (iPfT_debug_) {
        System.out.println(" potential next st links:");
        Iterator<BLink> links = nextStreetLinks.iterator();
        while (links.hasNext()) {
          BLink link = links.next();
          System.out.println("  - " + link.getID() + " det: " + calcDet(curLink, link));
        }
      }
      BLink chosenLink = null;
      if (nextStreetLinks.size() == 1) {
        chosenLink = (BLink) nextStreetLinks.get(0);
      } else if (nextStreetLinks.size() == 2) {
        BLink first = (BLink) nextStreetLinks.get(0);
        BLink last = (BLink) nextStreetLinks.get(nextStreetLinks.size() - 1);
        double detFirst = calcDet(curLink, first);
        double detLast = calcDet(curLink, last);

        if (nextDir.equals("left")) {
          chosenLink = (detFirst > detLast) ? first : last;
        } else if (nextDir.equals("right")) {
          chosenLink = (detFirst < detLast) ? first : last;
        } else {
          if (iPfT_debug_) {
            System.out.println(" error: 2 turn links found with no l/r direction");
          }
          return false;
        }
      } else {
        if (iPfT_debug_) {
          System.out.println(" error: more than 2 turn links found");
        }
        return false;
      }
      if (iPfT_debug_) {
        System.out.println(" chosen link: " + chosenLink.getID());
      }
      // add links to path
      Iterator<BLink> addLinks = newLinks.iterator();
      while (addLinks.hasNext()) {
        BLink addLink = addLinks.next();
        iPfT_links_.add(addLink);
        if (iPfT_debug_) {
          System.out.println(" # adding link: " + addLink.getID());
        }
      }
      // update tracking variables
      iPfT_origin_ = toNode;
      iPfT_curLink_ = chosenLink;
      return true;
    }

    // if we reach this point, we know we hit the limit
    if (iPfT_debug_) {
      System.out.println(" exceeded limit");
    }
    return false;
  }

  private double calcDet(BLink link1, BLink link2) {
    //Edge e1 = link1.getEdge(), e2 = link2.getEdge();
    BNode b = baseNet_.commonNode(link1, link2);

    BNode a = baseNet_.opposite(b, link1);
    //CNode b = (CNode) bv.element();
    BNode c = baseNet_.opposite(b, link2);

    return det3by3(a.getX(), a.getY(), 1, b.getX(), b.getY(), 1, c.getX(), c.getY(), 1);
  }

  private double det3by3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) {
    return a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1;
  }
}






















